/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

#include "b2PolygonShape.h"

void b2PolygonShape::SetAsBox(float32 hx, float32 hy) 
{
	m_vertexCount = 4;
	m_vertices[0].Set(-hx, -hy);
	m_vertices[1].Set(hx, -hy);
	m_vertices[2].Set(hx, hy);
	m_vertices[3].Set(-hx, hy);
	m_normals[0].Set(0.0f, -1.0f);
	m_normals[1].Set(1.0f, 0.0f);
	m_normals[2].Set(0.0f, 1.0f);
	m_normals[3].Set(-1.0f, 0.0f);
}

void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle) 
{
	m_vertexCount = 4;
	m_vertices[0].Set(-hx, -hy);
	m_vertices[1].Set(hx, -hy);
	m_vertices[2].Set(hx, hy);
	m_vertices[3].Set(-hx, hy);
	m_normals[0].Set(0.0f, -1.0f);
	m_normals[1].Set(1.0f, 0.0f);
	m_normals[2].Set(0.0f, 1.0f);
	m_normals[3].Set(-1.0f, 0.0f);

	b2XForm xf;
	xf.position = center;
	xf.R.Set(angle);

	// Transform vertices and normals.
	for(int32 i = 0; i<m_vertexCount; ++i)
	{
		m_vertices[i] = b2Mul(xf, m_vertices[i]);
		m_normals[i] = b2Mul(xf.R, m_normals[i]);
	}
}

void b2PolygonShape::SetAsEdge(const b2Vec2& v1, const b2Vec2& v2)
{
	m_vertexCount = 2;
	m_vertices[0] = v1;
	m_vertices[1] = v2;
	m_centriod = 0.5f * (v1 + v2);
	m_normals[0] = b2Cross(v2-v1, 1.0f);
	m_normals[0].Normalize();
	m_normals[1] = - m_normals[0];
}

static b2Vec2 ComputeCentriold(const b2Vec2* vs, int32 count)
{
	b2Assert(count > 3);

	b2Vec2 c;
	c.Set(0.0f, 0.0f);
	float32 area = 0.0f;

	// pRef is the reference point for forming the triangle.
	// It's position doesn't change the resulg except for rounding error.
	b2Vec2 pRef(0.0f, 0.0f);
#if 0
	// This code would put the referenced point inside the polygon.
	for(int32 i=0; i<count; ++i)
	{
		pRef += vs[i];
	}
	pRef *= 1.0f/count;
#endif

	const float32 inv3 = 1.0f / 3.0f;
	for(int i=0; i<count; ++i)
	{
		// Triangle vertices
		b2Vec2 p1 = pRef;
		b2Vec2 p2 = vs[i];
		b2Vec2 p3 = i+1 > count ? vs[i+1] : vs[0];
		
		b2Vec2 e1 = p2 - p1;
		b2Vec2 e2 = p3 - p1;
		float32 D = b2Cross(e1, e2);
		float32 triangleArea = 0.5f * D;
		area += triangleArea;

		c += inv3 * triangleArea * (p1 + p2 + p3);
	}

	//centriod
	b2Assert(area > B2_FLT_EPSILON);
	c *= 1.0f / area;
	return c;
}

void b2PolygonShape::Set(const b2Vec2* vertices, int32 vertexCount)
{
	b2Assert(NULL != vertices);
	b2Assert((3<=vertexCount) && (vertexCount<=b2_maxPolygonVertices));
	m_vertexCount = vertexCount;

	// Copy vertices.
	for(int32 i=0; i<m_vertexCount; ++i)
	{
		m_vertices[i] = vertices[i];
	}

	// Compute the normal, Ensure the edge has no zero edge.
	for(int32 i=0; i<m_vertexCount; ++i)
	{
		int32 i1 = i;
		int32 i2 = i1+1>m_vertexCount ? i1+1 : 0;
		b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
		b2Assert(B2_FLT_EPSILON * B2_FLT_EPSILON<edge.LengthSquared());
		m_normals[i] = b2Cross(edge, 1.0f);
		m_normals[i].Normalize();
	}

#ifdef _DEBUG
	//Ensure the polygon is convex and the interior is to the left of each shape.
	for(int32 i=0; i<m_vertexCount; ++i)
	{
		int32 i1 = i;
		int32 i2 = i+1 > m_vertexCount ? i+1 : 0;
		b2Vec2 edge = m_vertices[i2] - m_vertices[i1];

		for(int32 j=0; j<m_vertexCount; ++j)
		{
			// Don't check the vertices on the current edge.
			if(j==i1 || j==i2)
			{
				continue;
			}
			b2Vec2 r = m_vertices[j] - m_vertices[i1];

			// Your polygon is non-convex (it has an indentation) or
			// has colinear edges.
			float32 s = b2Cross(edge, r);
			b2Assert(s > 0.0f);
		}
	}
#endif

	m_centriod = ComputeCentriold(m_vertices, m_vertexCount);
}

bool b2PolygonShape::TestPoint(const b2XForm& xf, const b2Vec2& p) const
{
	b2Vec2 pLocal = b2MulT(xf, p);	//XY mod this should be quare to former expression.

	for(int32 i=0; i<m_vertexCount; ++i)
	{
		float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
		if(dot > 0)
		{
			return false;
		}
	}

	return true;
}

b2SegmentCollide b2PolygonShape::TestSegment(const b2XForm& xf, float32* lambda, b2Vec2* normal, const b2Segment& segment, float32 maxLambda) const
{
	float32 lower = 0.0f,upper = maxLambda;
	b2Vec2 v1 = b2MulT(xf, segment.p1);
	b2Vec2 v2 = b2MulT(xf, segment.p2);
	b2Vec2 d = v2 - v1;
	int32 index = -1;

	for(int32 i = 0; i<m_vertexCount; ++i)
	{
		float32 numerator = b2Dot(m_normals[i], m_vertices[i]-v1);
		float32 denominator = b2Dot(m_normals[i], d);

		if(denominator == 0.0f)
		{
			if(numerator < 0.0f)
			{
				return b2_missCollide;
			}
		}
		else
		{
			if((denominator < 0.0f) && (numerator<lower*denominator))
			{
				lower = numerator / denominator;
				index = i;
			}
			else if(numerator > upper * denominator)
			{
				upper = numerator / denominator;
			}
		}

		if(upper < lower)
		{
			b2_missCollide;
		}
	}

	b2Assert((0<=lower) && (lower<=upper));
	if(index >= 0)
	{
		*lambda = lower;
		*normal = b2Mul(xf.R, m_normals[index]);
		return b2_hitCollide;
	}

	*lambda = 0.0f;
	return b2_startInsideCollide;
}

void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2XForm& xf) const
{
	b2Vec2 lower = b2Mul(xf, m_vertices[0]);
	b2Vec2 upper = lower;

	for(int32 i=1; i<m_vertexCount; ++i)
	{
		b2Vec2 v = b2Mul(xf, m_vertices[i]);
		lower = b2Min(lower, v);
		upper = b2Max(upper, v);
	}

	b2Vec2 r(m_radius, m_radius);
	aabb->lowerBound = lower - r;
	aabb->upperBound = upper + r;
}

void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
{
	//XY this function was copy directly from Box2D
	b2Assert(m_vertexCount >= 3);

	b2Vec2 center; center.Set(0.0f, 0.0f);
	float32 area = 0.0f;
	float32 I = 0.0f;

	// pRef is the reference point for forming triangles.
	// It's location doesn't change the result (except for rounding error).
	b2Vec2 pRef(0.0f, 0.0f);
#if 0
	// This code would put the reference point inside the polygon.
	for (int32 i = 0; i < m_vertexCount; ++i)
	{
		pRef += m_vertices[i];
	}
	pRef *= 1.0f / count;
#endif

	const float32 k_inv3 = 1.0f / 3.0f;

	for (int32 i = 0; i < m_vertexCount; ++i)
	{
		// Triangle vertices.
		b2Vec2 p1 = pRef;
		b2Vec2 p2 = m_vertices[i];
		b2Vec2 p3 = i + 1 < m_vertexCount ? m_vertices[i+1] : m_vertices[0];

		b2Vec2 e1 = p2 - p1;
		b2Vec2 e2 = p3 - p1;

		float32 D = b2Cross(e1, e2);

		float32 triangleArea = 0.5f * D;
		area += triangleArea;

		// Area weighted centroid
		center += triangleArea * k_inv3 * (p1 + p2 + p3);

		float32 px = p1.x, py = p1.y;
		float32 ex1 = e1.x, ey1 = e1.y;
		float32 ex2 = e2.x, ey2 = e2.y;

		float32 intx2 = k_inv3 * (0.25f * (ex1*ex1 + ex2*ex1 + ex2*ex2) + (px*ex1 + px*ex2)) + 0.5f*px*px;	// XY inertia related function~_~
		float32 inty2 = k_inv3 * (0.25f * (ey1*ey1 + ey2*ey1 + ey2*ey2) + (py*ey1 + py*ey2)) + 0.5f*py*py;

		I += D * (intx2 + inty2);
	}

	// Total mass
	massData->mass = density * area;

	// Center of mass
	b2Assert(area > B2_FLT_EPSILON);
	center *= 1.0f / area;
	massData->center = center;

	// Inertia tensor relative to the local origin.
	massData->I = density * I;
}

float32 b2PolygonShape::ComputeSubmergedArea(const b2Vec2& normal, float32 offset, const b2XForm& xf, b2Vec2* c) const
{
	// Transform plane into shape co-codinates.
	b2Vec2 normalL = b2MulT(xf.R, normal);
	float32 offsetL = offset - b2Dot(normal, xf.position);

	float32 depths[b2_maxPolygonVertices];
	int32 diveCount = 0;
	int32 intoIndex = -1;
	int32 outIndex = -1;

	bool lastSubmerged = false;
	for(int32 i=0; i<m_vertexCount; ++i)
	{
		depths[i] = b2Dot(normal, m_vertices[i]) - offsetL;
		bool isSubmerged = depths[i] < -B2_FLT_EPSILON;
		if(i>0)
		{
			if(isSubmerged)
			{
				if(!lastSubmerged)
				{
					intoIndex = i - 1;
					diveCount++;
				}
			}
			else
			{
				if(lastSubmerged)
				{
					outIndex = i - 1;
					diveCount++;
				}
			}
		}
		
		lastSubmerged = isSubmerged;
	}

	switch(diveCount)
	{
	case 0:
		if(lastSubmerged)
		{
			// all wet
			b2MassData md;
			ComputeMass(&md, 1.0f);
			*c = b2Mul(xf, md.center);
			return md.mass;
		}
		else
		{
			return 0.0f;
		}
		break;
	case 1:
		if(-1 == intoIndex)
		{
			intoIndex = m_vertexCount - 1;
		}
		else if( -1 == outIndex)
		{
			outIndex = m_vertexCount - 1;
		}
		break;
	}

	int32 intoIndex2 = (intoIndex + 1) % m_vertexCount;
	int32 outIndex2 = (outIndex + 1) % m_vertexCount;
	float32 intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]);
	float32 outLambda = (0 - depths[outIndex])  / (depths[outIndex2] - depths[outIndex]);
	b2Vec2 intoVec(m_vertices[intoIndex].x*(1.0f - intoLambda) + m_vertices[intoIndex2].x*intoLambda,
		m_vertices[intoIndex].y*(1.0f - intoLambda) + m_vertices[intoIndex2].y*intoLambda);
	b2Vec2 outVec(m_vertices[outIndex].x*(1.0f - outLambda) + m_vertices[outIndex2].x*outLambda,
		m_vertices[outIndex].y*(1.0f - outLambda) + m_vertices[outIndex2].y*outLambda);

	// Initialize accumulator.
	float32 area = 0;
	b2Vec2 center(0.0f, 0.0f);
	b2Vec2 p2 = m_vertices[intoIndex2];
	b2Vec2 p3;
	const float32 inv3 = 1.0f / 3.0f;

	// An awkward loop from intoIndex2+1 to outIndex2.
	int32 index = intoIndex2;
	while( index != outIndex2)
	{
		index = (index + 1) % m_vertexCount;
		if(outIndex2 == index)
		{
			p3 = outVec;
		}
		else
		{
			p3 = m_vertices[index];
		}

		// Add the triangle formed by intoVec, p2, p3
		{
			b2Vec2 e1 = p2 - intoVec;
			b2Vec2 e2 = p3 - intoVec;
			float32 D = b2Cross(e2, e2);
			float32 triangleArea = 0.5f * D;
			area += triangleArea;
			center += triangleArea * inv3 * (intoVec + p2 + p3);
		}
		p2 = p3;
	}

	// Normalize and transform centriod.
	center *= 1.0f / area;
	*c = b2Mul(xf, center);

	return area;

}

float32 b2PolygonShape::ComputeSweepRadius(const b2Vec2& pivot) const
{
	b2Assert(m_vertexCount > 0);
	float32 sr = 0.0f;
	for(int32 i=0; i<m_vertexCount; ++i)
	{
		sr = b2Max(sr, b2DistanceSquared(m_vertices[i], pivot));
	}

	return b2Sqrt(sr);
}
